#include "src/math/transformation.h"
#include <iostream>
#include <cmath>

Transformation::Transformation(){
    localToWorld = Matrix(4,4, IDENTITY_MATRIX);
    worldToLocal = Matrix(4,4, IDENTITY_MATRIX);
}

Transformation::~Transformation(){
}

const Matrix &Transformation::getLocalToWorld() const{
    return localToWorld;
}

const Matrix &Transformation::getWorldToLocal() const{
    return worldToLocal;
}

Matrix Transformation::getLocalToWorldDirection() const{
    Matrix result(3,3);
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            result[i][j] = localToWorld[i][j];
        }
    }
    return result;
}

Matrix Transformation::getWorldToLocalDirection() const{
    Matrix result(3,3);
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            result[i][j] = worldToLocal[i][j];
        }
    }
    return result;
}

Matrix Transformation::addTranslation(const Vector3D &_translationVector){
    Matrix translation(4,4, IDENTITY_MATRIX);
    Matrix translationInverse(4,4, IDENTITY_MATRIX);

    for(int i = 0; i < 3; i++){
        translation[i][3] = _translationVector[i];
        translationInverse[i][3] = -_translationVector[i];
    }
//
//    localToWorld = localToWorld * translation;
//    worldToLocal = translationInverse * worldToLocal;
    localToWorld = translation * localToWorld;
    worldToLocal = worldToLocal * translationInverse;
    return translation;
}

Matrix Transformation::addRotation(double _tetha, const Vector3D &_rotationAxis, const Vector3D &_center, const Vector3D & _relativeVector){
    bool translate = false;
    Matrix result(4,4, IDENTITY_MATRIX);

    if(!(fabs(_rotationAxis.length_2() - 1.0) < ERROR ))
        std::cout <<"Coloque o vetor de rotação unitátio! " << _rotationAxis.length_2() <<std::endl;

    result = addTranslation(-_center);

    if(_relativeVector.length_2() > ERROR){
        translate = true;
        result = addTranslation(_relativeVector) * result;

//        std::cout <<"Transladou dentor da rotação!" <<std::endl;
    }

    Matrix rotation(4,4, IDENTITY_MATRIX);
    Matrix rotationInverse(4,4, IDENTITY_MATRIX);


    double u = _rotationAxis.x;
    double u2 = u*u;
    double v = _rotationAxis.y;
    double v2 = v*v;
    double w = _rotationAxis.z;
    double w2 = w * w;
    double c = cos(_tetha);
    double s = sin(_tetha);

    rotation[0][0] = u2 + ((1 - u2) * c);
    rotation[0][1] = (u * v * (1 - c)) - (w * s);
    rotation[0][2] = (u * w * (1 - c)) + (v * s);

    rotation[1][0] = (u * v * (1 - c)) + (w * s);
    rotation[1][1] = v2 + ((1 - v2) * cos(_tetha));
    rotation[1][2] = (v * w * (1 - c)) - (u * s);

    rotation[2][0] = (u * w * (1 - c)) - (v * s);
    rotation[2][1] = (v * w * (1 - c)) + (u * s);
    rotation[2][2] = w2 + ((1 - w2) * cos(_tetha));

    c = cos(-_tetha);
    s = sin(-_tetha);

    rotationInverse[0][0] = u2 + ((1 - u2) * c);
    rotationInverse[0][1] = (u * v * (1 - c)) - (w * s);
    rotationInverse[0][2] = (u * w * (1 - c)) + (v * s);

    rotationInverse[1][0] = (u * v * (1 - c)) + (w * s);
    rotationInverse[1][1] = v2 + ((1 - v2) * cos(_tetha));
    rotationInverse[1][2] = (v * w * (1 - c)) - (u * s);

    rotationInverse[2][0] = (u * w * (1 - c)) - (v * s);
    rotationInverse[2][1] = (v * w * (1 - c)) + (u * s);
    rotationInverse[2][2] = w2 + ((1 - w2) * cos(_tetha));

    result = rotation * result;

    localToWorld = rotation * localToWorld;
    worldToLocal = worldToLocal * rotationInverse;

    if(translate){
        result = addTranslation(-_relativeVector) * result;
    }
    result = addTranslation(_center) * result;

    return result;
}

Matrix Transformation::addScale(const Vector3D &_scaleVector, const Vector3D &_center, const Vector3D & _relativeVector){
    bool translate = false;
    Matrix result(4,4, IDENTITY_MATRIX);



    result = addTranslation(-_center);

    if(_relativeVector.length_2() > ERROR){
        translate = true;
        result = addTranslation(_relativeVector) * result;
//        std::cout <<"Transladou dentro da escala!" <<std::endl;
    }

    Matrix scale(4,4, IDENTITY_MATRIX);
    Matrix scaleInverse(4,4, IDENTITY_MATRIX);

    for(int i = 0; i < 3; i++){
        scale[i][i] = _scaleVector[i];
        scaleInverse[i][i] = 1.0/_scaleVector[i];
    }

    result = scale * result;

    localToWorld = scale * localToWorld;
    worldToLocal = worldToLocal * scaleInverse;

    if(translate){
        result = addTranslation(-_relativeVector) * result;
    }

    result = addTranslation(_center) * result;

    return result;
}
